#pragma config(Sensor, in1,    pot,                 sensorPotentiometer)
#pragma config(Sensor, in7,    JumperTwo,           sensorPotentiometer)
#pragma config(Sensor, in8,    JumperOne,           sensorPotentiometer)
#pragma config(Sensor, dgtl1,  rshaft,              sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  rightlimit,          sensorTouch)
#pragma config(Sensor, dgtl4,  leftlimit,           sensorTouch)
#pragma config(Sensor, dgtl9,  ultra,               sensorSONAR_inch)
#pragma config(Sensor, dgtl11, lshaft,              sensorQuadEncoder)
#pragma config(Motor,  port1,           leftArm1,      tmotorNormal, openLoop)
#pragma config(Motor,  port3,           leftArm2,      tmotorNormal, openLoop)
#pragma config(Motor,  port5,           leftDB,        tmotorNormal, openLoop)
#pragma config(Motor,  port6,           rightDB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           intake,        tmotorNormal, openLoop)
#pragma config(Motor,  port8,           rightArm1,     tmotorNormal, openLoop)
#pragma config(Motor,  port10,          rightArm2,     tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//DO NOT MODIFY ABOVE!!! (Configuration preprocessor directives)

/*
 * Team D VEX main and driver control competition code.
 * @author Derek Leung and Michael Searing
 */

//Required code - do not modify!!
#pragma platform(VEX)
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#include "Vex_Competition_Includes.c"

//Debugger disables robot autonomous power
bool debug = false;

//Arm constants
const int ARM_BOTTOM = 875;
const int ARM_MAXERR = 12;
const int ARM_30 = 2680; //for scoring on 30" goals
const int ARM_20 = 2190; //for scoring on 20" goals
const int ARM_10 = 1435; //for intaking stack of two objects
const int ARM_6 = 1100; //for intaking single object
const float ARM_PMULT = 0.5; //multiplier for P in PID
const float ARM_DMULT = 0.4; //multiplier for D in PID
const float ARM_IMULT = 2.0; //multiplier for I in PID
const float DRIVE_CORRECTION = 0.5;
const float DRIVE_PMULT = 0.38;
const float DRIVE_DMULT = 2.2;
const int DRIVE_APPROACH_ERR = 35;
const int DRIVE_TURN_ERR = 200;

int objects;

bool arm_move=false;
bool intake_move=false;
bool auton=false;

void pre_auton() {}
#include "d254x.c" //Autonomous functions

//////////////////////////////////
/////// Autonomous Control ///////
//////////////////////////////////
/**
 * Programming skills control.
 */
void programmingskills()
{
  auton = true;

  //intake 2 in front of 11
  objects = 2;
  regulate_arm(ARM_10);
  objects = 4;
  while(SensorValue(leftlimit) == 0 && SensorValue(rightlimit) == 0) {;}
  set_intake(127);
  set_drivepower(30);
  while(SensorValue(pot) > ARM_BOTTOM)
  set_armpower(-60);
  set_armpower(0);
  set_drivepower(0);
  wait1Msec(1000);
  set_intake(0);
  while(SensorValue(pot) < 1650)
  set_armpower(127);
  set_armpower(20);
  while(SensorValue(leftlimit) == 0 && SensorValue(rightlimit) == 0) {}
  wait1Msec(500);
  //outtake into 11
  set_intake(-127);
  while(SensorValue(leftlimit) == 0 && SensorValue(rightlimit) == 0) {}
  set_intake(127);
  wait1Msec(500);
  set_intake(0);
  while(SensorValue(leftlimit) == 0 && SensorValue(rightlimit) == 0) {}
  while(SensorValue(pot) < 2000)
  set_armpower(127);
  set_intake(127);

  //matchload 4
  set_armpower(30);
  while(SensorValue(rightlimit) == 0 && SensorValue(leftlimit) == 0) {}
  set_intake(0);
  set_drivepower(50);
  while(SensorValue(pot) > 1100)

  //lower arm for drive
  set_armpower(-50);
  set_armpower(25);
  set_drivepower(127);
  while(SensorValue(rightlimit) == 0 && SensorValue(leftlimit) == 0) {}
  set_drivepower(0);
  set_armpower(-40);
  wait1Msec(500);
  objects = 5;
  regulate_arm(ARM_6);
  regulate_arm(ARM_6);
  set_intake(127);
  while(SensorValue(rightlimit) == 0 && SensorValue(leftlimit) == 0) {;}

  //intake doubler
  set_drivepower(35);
  wait1Msec(400);
  regulate_arm(ARM_BOTTOM);
  set_drivepower(0);
  wait1Msec(400);
  while(SensorValue(pot) < 1100)
    set_armpower(127);
  set_armpower(30);
  set_intake(90);

  //push stack
  while(SensorValue(rightlimit) == 0 && SensorValue(leftlimit) == 0) {}
  regulate_drive(1,30,127,0);
  wait1Msec(500);
  regulate_drive(-1,34,127,0);

  //outtake into 30
  while(SensorValue(pot) < 2900)
    set_armpower(127);
  set_armpower(30);
  while(SensorValue(rightlimit) == 0 && SensorValue(leftlimit) == 0) {}
  set_intake(-127);
  wait1Msec(2000);
  while(SensorValue(rightlimit) == 0 && SensorValue(leftlimit) == 0) {}
  set_intake(0);
  set_armpower(0);
  wait1Msec(1000);

  //drive to 20 and intake
  while(SensorValue(rightlimit) == 0 && SensorValue(leftlimit) == 0) {}
  while(SensorValue(pot) > 1300)
    set_armpower(-50);
  objects = 0;
  regulate_arm(ARM_10);
  set_drivepower(50);
  wait1Msec(2000);
  set_drivepower(30);
  set_intake(127);
	regulate_arm(ARM_BOTTOM);
	wait1Msec(500);
	regulate_drive(-1,10,90,0);
	objects = 2;
	regulate_arm(ARM_20);
	regulate_drive(1,11,90,0);
	wait1Msec(500);

	//outtake into 20
	set_intake(-60);
	wait1Msec(2000);
	set_intake(127);
	set_drivepower(65);
	wait1Msec(1300);
	set_intake(-127);
  wait1Msec(2000);
  set_intake(0);
  set_drivepower(-100);
  wait1Msec(1500);
  set_armpower(0);
  set_drivepower(0);
}

/**
 * Autonomous control when starting in isolation zone.
 */
void isolation()
{
  auton = true;
  while(SensorValue(rightlimit) == 0 && SensorValue(leftlimit) == 0) {;}
	set_intake(127);
	regulate_arm(ARM_6);
	wait1Msec(100);
	regulate_drive(1,9,127,0);        //driving to barrel
	regulate_arm(ARM_BOTTOM);
	wait1Msec(200);
  while(SensorValue(pot) < 1300)
    set_armpower(60);

  //raise to 20
	while(SensorValue(pot) < 1700) {set_armpower(127);}
  while(SensorValue(pot) < 2100) {set_armpower(64);}
  set_armpower(32);

	regulate_drive(1,30,127,0);
	set_drivepower(127);
	wait1Msec(1000);
	set_armpower(20);
	set_drivepower(127);
	wait1Msec(700);
	set_drivepower(0);
	set_intake(-127);
	wait1Msec(2000);
	set_intake(127);
	regulate_drive(-1,10,127,0);
	wait1Msec(300);
	set_intake(-127);
	wait1Msec(3000);
	set_intake(0);
	regulate_drive(-1,25,127,0);
	set_drivepower(0);
	set_armpower(0);
}

/**
 * Autonomous control when starting in interaction zone.
 */
void interaction()
{
  regulate_arm(ARM_30);
  regulate_drive(1,48,127,0);       //Drive forward to 30"
  set_intake(-1);
}

//////////////////////////
/////// Autonomous ///////
//////////////////////////
task autonomous()
{
  if(SensorValue(JumperOne) == 0) //Jumper in analog 8
    programmingskills();
  else if(SensorValue(JumperTwo) == 0) //Jumper in analog 7
    interaction();
  else
    isolation();
}



//////////////////////////////
/////// Driver Control ///////
//////////////////////////////
task usercontrol()
{
  auton = false;
  StartTask(oparm);
  while(true)
  {
    //motor[leftDB] = -vexRT[Ch3] - vexRT[Ch1];
    //motor[rightDB] = -vexRT[Ch3] + vexRT[Ch1];
    motor[leftDB] = -vexRT[Ch3];
    motor[rightDB] = -vexRT[Ch2];
    int debugarm = vexRT[Ch3Xmtr2];
    if(!arm_move || debug)
    {
      motor[leftArm1] = vexRT[Ch3Xmtr2];
      motor[leftArm2] = vexRT[Ch3Xmtr2] * -1;
      motor[rightArm1] = vexRT[Ch3Xmtr2];
      motor[rightArm2] = vexRT[Ch3Xmtr2] * -1;
    }
    if(!intake_move || debug)
      motor[intake] = vexRT[Ch2Xmtr2];
  }
}
